/**************************************************************************
File:        positioner.cpp
Description:

Copyright:   (C) 2001-2012 by ZJ Laczik
Email:       Zsolt.Laczik@eng.ox.ac.uk
****************************************************************************
Change log:
   begin      Mon Mar 19 2001
   15/2/2012	QT4 update
***************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>

#include <QApplication>
#include <QString>

//#include "stepper.h"

#include "positioner.h"

#define X_AXIS 1
#define Y_AXIS 2
#define Z_AXIS 3

#define P_TYPE_DUMMY	0
#define	P_TYPE_IP28	1
#define P_TYPE_PP	2

Positioner::Positioner(QWidget *parent) : QObject(parent) {

//?/     if (IP28Init()) {
//?/         mType = P_TYPE_IP28;
//?/         fprintf(stderr,"IP28 set OK\n");
//?/     }
//?/     else
    if ( true ) {
        mType = P_TYPE_PP ;
    }
    else if (DummyInit()) {
        mType = P_TYPE_DUMMY;
    }
    else
        exit(1);

    mStopFlag = false;
    stepper.SetStopFlagPointer(&mStopFlag) ;

    connect(&stepper,		SIGNAL(Position(float, float, float)),
            SIGNAL(Position(float, float, float)));

}

Positioner::~Positioner() {
    if (mType == P_TYPE_IP28)
        IP28Shutdown();
}


bool Positioner::IP28Init() {
    QString cmd, response;
    bool success;

    // temporary disable
    return false ;

    // init serial port connection to IP28
//fprintf(stderr,"creating serport object\n");
    if (NULL == (mSerPort = new QextSerialPort("/dev/ttyS1", QextSerialPort::Polling)))
        // QextSerialPort::EventDriven generates high CPU load (see http://www.qtcentre.org/threads/38986-100-CPU-load)
        return false ;
//fprintf(stderr,"opening serport\n");
    if (!mSerPort->open(QIODevice::ReadWrite))
        return false;
    mSerPort->setBaudRate(BAUD2400);
    mSerPort->setDataBits(DATA_8);
    mSerPort->setParity(PAR_NONE);
    mSerPort->setStopBits(STOP_2);
    mSerPort->setFlowControl(FLOW_OFF);
    mSerPort->setTimeout(0);
    // init. IP28 positioning system
//fprintf(stderr,"sendcmd 1\n");
    success = IP28SendCmd("FA33", response);					// display everything except line numbers
    if (!success)	return false;
    success = IP28SendCmd("FP83", response);					// ramp2 (fast) + full acceleration table, full steps
    if (!success)	return false;
    mStepRate = 4000;
    cmd.sprintf("RA%d",mStepRate);
    success = IP28SendCmd(cmd,	response);						// set step rate
    if (!success)	return false;
    success = IP28SendCmd("AA",	response);						// set floating origin to zero
    if (!success)	return false;

    success = IP28SendCmd("C1",	response);
    if (!success)	return false;
    success = IP28SendCmd("C2",	response);
    if (!success)	return false;
    success = IP28SendCmd("C3",	response);
    if (!success)	return false;
    success = IP28SendCmd("C4",	response);
    if (!success)	return false;
    success = IP28SendCmd("C5",	response);
    if (!success)	return false;
    success = IP28SendCmd("C6",	response);
    if (!success)	return false;
    success = IP28SendCmd("C7",	response);
    if (!success)	return false;
    success = IP28SendCmd("C8",	response);
    if (!success)	return false;

    // get and store current position
    int x, y, z, fx, fy, fz;
    success = IP28ReadPosition(x, y, z, fx, fy, fz);
    if (!success)	return false;
    mX = float(fx)/10.0;
    mY = float(fy)/10.0;
    mZ = float(fz)/10.0;
    return true;
}

void Positioner::IP28Shutdown() {
    bool success;
    QString response;

    success = IP28SendCmd("C1",	response);
    success = IP28SendCmd("C2",	response);
    success = IP28SendCmd("C3",	response);
    success = IP28SendCmd("C4",	response);
    success = IP28SendCmd("C5",	response);
    success = IP28SendCmd("C6",	response);
    success = IP28SendCmd("C7",	response);
    success = IP28SendCmd("C8",	response);

    mSerPort->close();
}

bool Positioner::PPMoveTo(float x, float y, float z) {
    int xx,yy,zz;

    // calc positions in 0.1 micron units
    xx = int(rint(x*10.0));
    yy = int(rint(y*10.0));
    zz = int(rint(z*10.0));

    // do the moves
    if (!mStopFlag)
        stepper.StepAbs(xx, X_AX) ;
    if (!mStopFlag)
        stepper.StepAbs(yy, Y_AX) ;
    if (!mStopFlag)
        stepper.StepAbs(zz, Z_AX) ;

    mX = float(stepper.GetPos(X_AXIS))/10.0;
    mY = float(stepper.GetPos(Y_AXIS))/10.0;
    mZ = float(stepper.GetPos(Z_AXIS))/10.0;

    emit(Position(mX,mY,mZ));
    qApp->processEvents();

    mStopFlag = false ;
    emit( Running(false) );
    return true;
}

bool Positioner::IP28ReadPosition(int &x, int &y, int &z, int &fx, int &fy, int &fz) {
    bool success;
    QString pos;
    int idx;
    // get position values from IP28
    success = IP28SendCmd("DA",	pos);
    if (!success)	return false;
    // parse response
    idx = 0;
    // ignore W and FW
    idx = pos.indexOf('=',idx) + 1;
    idx = pos.indexOf('=',idx) + 1;
    // this ought to be X
    idx = pos.indexOf('=',idx) + 1;
    x = pos.mid(idx,8).toInt();
    idx = pos.indexOf('=',idx) + 1;
    fx = pos.mid(idx,8).toInt();
    idx = pos.indexOf('=',idx) + 1;
    y = pos.mid(idx,8).toInt();
    idx = pos.indexOf('=',idx) + 1;
    fy = pos.mid(idx,8).toInt();
    idx = pos.indexOf('=',idx) + 1;
    z = pos.mid(idx,8).toInt();
    idx = pos.indexOf('=',idx) + 1;
    fz = pos.mid(idx,8).toInt();
    return true;
}

bool Positioner::IP28MoveTo(float x, float y, float z) {
    bool success = false;
    int xto,yto,zto;
    int xx,yy,zz;
    int d;
    QString cmd,response;
//fprintf(stderr,"start IP28MoveTo: %f %f %f\n",x,y,z);
    // calc positions in 0.1 micron units
    xx = int(rint(x*10.0));
    yy = int(rint(y*10.0));
    zz = int(rint(z*10.0));
    // calc. timeout values
    xto = 1000 + 2*1000*int(10.0*fabs(x-mX))/mStepRate;
    yto = 1000 + 2*1000*int(10.0*fabs(y-mY))/mStepRate;
    zto = 1000 + 2*1000*int(10.0*fabs(z-mZ))/mStepRate;
//fprintf(stderr,"IP28MoveTo: mStopFlag=%d\n",mStopFlag);
    // do the moves
    cmd.sprintf("PX%d",xx);
    if (!mStopFlag)
        success = IP28SendCmd(cmd,	response, true, xto);
    if (!success) {
//fprintf(stderr,"IP28MoveTo: X move failed\n");
        // generate 'emergency stop'
        IP28SendCmd("?",	response);
    }
    cmd.sprintf("PY%d",yy);
    if (!mStopFlag)
        success = IP28SendCmd(cmd,	response, true, yto);
    if (!success) {
//fprintf(stderr,"IP28MoveTo: Y move failed\n");
        // generate 'emergency stop'
        IP28SendCmd("?",	response);
    }
    cmd.sprintf("PZ%d",zz);
    if (!mStopFlag)
        success = IP28SendCmd(cmd,	response, true, zto);
    if (!success) {
//fprintf(stderr,"IP28MoveTo: Z move failed\n");
        // generate 'emergency stop'
        IP28SendCmd("?",	response);
    }
    // get rid of any error messages
    IP28SendCmd("?",	response);
    IP28SendCmd("?",	response);
    // update positions
    success = IP28ReadPosition(d, d, d, xx, yy, zz);
    if (!success)	return false;
    mX = float(xx)/10.0;
    mY = float(yy)/10.0;
    mZ = float(zz)/10.0;
//fprintf(stderr,"IP28MoveTo finished OK\n");
    return true;
}

bool Positioner::IP28SendCmd(QString cmd, QString & response, bool wait, int timeout) {
    QString ts;
    int l;
    int cnt = 0;
    int ch = 0;

    fprintf(stderr,"IP28SendCmd cmd: %s;<CR><LF>\n", cmd.toUtf8().constData());
    cmd += ";\r\n";

    response = "";
    l = cmd.length();
//?/    if (l != mSerPort->writeBlock(cmd.toUtf8().constData(), cmd.length() ))
//?/        return false;

//usleep(500000);
//fprintf(stderr,"IP28 sendcmd: waiting for response\n");
    if (wait) {
        while (ch != '*') {
//?/            ch = mSerPort->getch();
            if (ch>0) {
//fprintf(stderr,"IP28 sendcmd: char read 0x%x\n", ch);
                if (ch == 0x07) {
                    // error beep
                    // ignore
                }
                else if (ch == 0x0d) {
                    // CR
                    // ignore
                }
                else if (ch == 0x0a) {
                    // LF
                    // ignore
                    //response += '|';
                }
                if (ch < ' ') {
                    // non-printable char
                    // ignore
                    // ts.sprintf("\\0x%02x",ch);
                    // response += ts;
                }
                else {
                    // add to the rest
                    response += ch;
                }
            }
            else {
//fprintf(stderr,"IP28 sendcmd: no char in buffer %d\n", cnt);
                usleep(100000);
                if (cnt>9 && !(cnt%5)) qApp->processEvents();
                cnt++;
                if (mStopFlag || (cnt>(timeout/100))) return false;				// 5 sec. timeout
            }
        }
    }
    fprintf(stderr,"IP28SendCmd response: %s\n", response.toUtf8().constData());
//usleep(1000000L);
    return true;
}


bool Positioner::DummyInit() {
    // get and store current position
    mX = mY = mZ = 0.0;
    return true;
}

void Positioner::Stop() {
    mStopFlag = true;
}

void Positioner::MoveTo(float x, float y, float z) {
    float s;
    QString str;

    emit( Running(true) );

    str.sprintf("Positioner: Moving to (%g,%g,%g)",x,y,z);
    emit( StatusMsg(str, true) );

    if (mType == P_TYPE_IP28) {
        IP28MoveTo(x,y,z);
        emit(Position(mX,mY,mZ));
        qApp->processEvents();
    }
    else if (mType == P_TYPE_PP) {
        PPMoveTo(x,y,z);
        emit(Position(mX,mY,mZ));
        qApp->processEvents();
    }
    else {
        // this is just a dummy
        s = (x-mX)/10.0;
        while (fabs(mX-x)>0.05) {
            mX += s;
            usleep(100000);
            emit(Position(mX,mY,mZ));
            qApp->processEvents();
            if (mStopFlag) break;
        }
        s = (y-mY)/10.0;
        while (fabs(mY-y)>0.05) {
            mY += s;
            usleep(100000);
            emit(Position(mX,mY,mZ));
            qApp->processEvents();
            if (mStopFlag) break;
        }
        s = (z-mZ)/10.0;
        while (fabs(mZ-z)>0.05) {
            mZ += s;
            usleep(100000);
            emit(Position(mX,mY,mZ));
            qApp->processEvents();
            if (mStopFlag) break;
        }
    }

    mStopFlag = false;
    emit( Running(false) );
}

void Positioner::MoveXTo(float pos) {
    MoveTo(pos, mY, mZ);
}

void Positioner::MoveYTo(float pos) {
    MoveTo(mX, pos, mZ);
}

void Positioner::MoveZTo(float pos) {
    MoveTo(mX, mY, pos);
}

void Positioner::Move(int axis, float movement) {
    float s,x,y,z;
    QString str;

    emit( Running(true) );

    str.sprintf("Positioner: Moving along axis%d by %g",axis,movement);
    emit( StatusMsg(str, true) );

    if (mType == P_TYPE_IP28) {
        x = mX;
        y = mY;
        z = mZ;
        switch (axis) {
        case X_AXIS:
            x += movement;
            break;
        case Y_AXIS:
            y += movement;
            break;
        case Z_AXIS:
            z += movement;
            break;
        }
        IP28MoveTo(x,y,z);
        emit(Position(mX,mY,mZ));
        qApp->processEvents();
    }
    else if (mType == P_TYPE_PP) {
        x = mX;
        y = mY;
        z = mZ;
        switch (axis) {
        case X_AXIS:
            x += movement;
            break;
        case Y_AXIS:
            y += movement;
            break;
        case Z_AXIS:
            z += movement;
            break;
        }
        PPMoveTo(x,y,z);
        emit(Position(mX,mY,mZ));
        qApp->processEvents();
    }
    else {
        // this is just a dummy
        switch (axis) {
        case X_AXIS:
            x = mX+movement;
            s = movement/10.0;
            while (fabs(mX-x)>0.05) {
                mX += s;
                usleep(100000);
                emit(Position(mX,mY,mZ));
                qApp->processEvents();
                if (mStopFlag) break;
            }
            break;
        case Y_AXIS:
            y = mY+movement;
            s = movement/10.0;
            while (fabs(mY-y)>0.05) {
                mY += s;
                usleep(100000);
                emit(Position(mX,mY,mZ));
                qApp->processEvents();
                if (mStopFlag) break;
            }
            break;
        case Z_AXIS:
            z = mZ+movement;
            s = movement/10.0;
            while (fabs(mZ-z)>0.05) {
                mZ += s;
                usleep(100000);
                emit(Position(mX,mY,mZ));
                qApp->processEvents();
                if (mStopFlag) break;
            }
            break;
        }
    }

    mStopFlag = false;
    emit( Running(false) );
}

void Positioner::InterLockOK(bool & success) {
    QString response;
    success = IP28SendCmd("UE2F",	response, true, 500);
    if (!success) {
        IP28SendCmd("?",	response);
        IP28SendCmd("?",	response);
    }
}

void Positioner::WarningSign(bool & success, bool on) {
    QString response;
    if (on) {
        success = IP28SendCmd("S1",	response, true, 500);
    }
    else {
        success = IP28SendCmd("C1",	response, true, 500);
    }
    if (!success) {
        IP28SendCmd("?",	response);
        IP28SendCmd("?",	response);
    }
}

void Positioner::Shoot(bool & success) {
    QString response;
    success = IP28SendCmd("<S4;C4;>",	response, true, 500);
    if (!success) {
        IP28SendCmd("?",	response);
        IP28SendCmd("?",	response);
    }
}



